#include "main.hpp"

using namespace std;

LinuxSerialDevice::LinuxSerialDevice(const char* device, uint32_t baudrate)
{
  m_device   = device;
  m_baudrate = baudrate;
}

LinuxSerialDevice::~LinuxSerialDevice()
{
  _serialClose();
}

void
LinuxSerialDevice::init()
{
//   DSTATUS("Attempting to open device %s with baudrate %u...\n", m_device,
//           m_baudrate);
std::cout << "Attempting to open device "<<m_device<<std::endl;
std::cout << "Attempting to open wtih baudrate  "<<m_baudrate<<std::endl;
  if (_serialStart(m_device, m_baudrate) < 0)
  {
    _serialClose();
    // DERROR("...Failed to start serial\n");
    std::cout << "..Failed to start serial\n" << std::endl;
    deviceStatus = false;
  }
  else
  {
    // DSTATUS("...Serial started successfully.\n");
    std::cout << "...Serial started successfully.\n" << std::endl;
    deviceStatus = true;
  }
}


void
LinuxSerialDevice::initOdd()
{
//   DSTATUS("Attempting to open device %s with baudrate %u...\n", m_device,
//           m_baudrate);
  if (_serialStartOdd(m_device, m_baudrate) < 0)
  {
    _serialClose();
    // DERROR("...Failed to start serial\n");
      std::cout << "..Failed to start serial\n" << std::endl;
    deviceStatus = false;
  }
  else
  {
    // DSTATUS("...Serial started successfully.\n");
    std::cout << "...Serial started successfully.\n" << std::endl;
    deviceStatus = true;
  }
}

bool
LinuxSerialDevice::getDeviceStatus()
{
  return deviceStatus;
}

// DJI::OSDK::time_ms
// LinuxSerialDevice::getTimeStamp()
// {
//   struct timeval tv = {0};

//   gettimeofday(&tv, NULL);

//   return (tv.tv_sec*1000 + tv.tv_usec/1000);
//   //return (uint32_t)time(NULL);
// }

size_t
LinuxSerialDevice::send(const uint8_t* buf, size_t len)
{
  return _serialWrite(buf, len);
}

size_t
LinuxSerialDevice::readall(uint8_t* buf, size_t maxlen)
{
  return _serialRead(buf, maxlen);
}

/*! Implement functions specific to this hardware driver */

/****
  The next few functions set serial port I/O parameters and implement serial R/W
functions.
  Implement termios-based serial i/o.
****/
void
LinuxSerialDevice::setBaudrate(uint32_t baudrate)
{
  m_baudrate = baudrate;
}

void
LinuxSerialDevice::setDevice(const char* device)
{
  m_device = device;
}

int
LinuxSerialDevice::setSerialPureTimedRead()
{
  return _serialConfig(m_baudrate, 8, 'N', 1, true);
}

int
LinuxSerialDevice::unsetSerialPureTimedRead()
{
  return _serialConfig(m_baudrate, 8, 'N', 1, false);
}

int
LinuxSerialDevice::serialRead(uint8_t* buf, int len)
{
  return _serialRead(buf, len);
}

int LinuxSerialDevice::_checkBaudRate(uint8_t (&buf)[BUFFER_SIZE])
{
//   int lengthForCheck   = 200;
//   int timeoutInSeconds = 2;

//   struct timespec curTime, absTimeout;
//   // Use clock_gettime instead of getttimeofday for compatibility with POSIX
//   // APIs
//   clock_gettime(CLOCK_REALTIME, &curTime);
//   absTimeout.tv_sec  = curTime.tv_sec + timeoutInSeconds;
//   absTimeout.tv_nsec = curTime.tv_nsec;

//   int receivedBytes = _serialRead(buf, lengthForCheck);

//   while (curTime.tv_sec < absTimeout.tv_sec)
//   {
//     if (receivedBytes < lengthForCheck)
//       receivedBytes +=
//         _serialRead(buf + receivedBytes, lengthForCheck - receivedBytes);
//     else
//       break;

//     clock_gettime(CLOCK_REALTIME, &curTime);
//   }
//   if (curTime.tv_sec >= absTimeout.tv_sec)
//     return -1;
//   if (buf + BUFFER_SIZE == std::find(buf, buf + BUFFER_SIZE, 0xAA))
//     return -2;

  return 1;
}

bool
LinuxSerialDevice::_serialOpen(const char* dev)
{
#ifdef __arm__
  m_serial_fd = open(dev, O_RDWR | O_NONBLOCK);
#elif __x86_64__
  m_serial_fd = open(dev, O_RDWR | O_NOCTTY);
#else
  m_serial_fd = open(dev, O_RDWR | O_NOCTTY);
#endif
  if (m_serial_fd < 0)
  {
    // DERROR("cannot open device %s\n", dev);
    std::cout << "cannot open device\n" << std::endl;
    return false;
  }
  return true;
}

bool
LinuxSerialDevice::_serialClose()
{
  close(m_serial_fd);
  m_serial_fd = -1;
  return true;
}

bool
LinuxSerialDevice::_serialFlush()
{
  if (m_serial_fd < 0)
  {
    // DERROR("flushing fail because no device is opened\n");
    std::cout << "flushing fail because no device is opened\n" << std::endl;
    return false;
  }
  else
  {
    tcflush(m_serial_fd, TCIFLUSH);
    return true;
  }
}

bool
LinuxSerialDevice::_serialConfig(int baudrate, char data_bits, char parity_bits,
                                 char stop_bits, bool testForData)
{
  int st_baud[] = { B4800,  B9600,   B19200,  B38400,
                    B57600, B115200, B230400, B921600, B1000000};
  int std_rate[] = { 4800,   9600,   19200,   38400,   57600,  115200,
                     230400, 921600, 1000000};

  int            i, j;
  struct termios newtio, oldtio;
  /* save current port parameter */
  if (tcgetattr(m_serial_fd, &oldtio) != 0)
  {
    // DERROR("fail to save current port\n");
    return false;
  }
  memset(&newtio, 0, sizeof(newtio));

  /* config the size of char */
  newtio.c_cflag |= CLOCAL | CREAD;
  newtio.c_cflag &= ~CSIZE;

  /* config data bit */
  switch (data_bits)
  {
    case 7:
      newtio.c_cflag |= CS7;
      break;
    case 8:
      newtio.c_cflag |= CS8;
      break;
    default:
      newtio.c_cflag |= CS8;
      break; //8N1 default config
  }
  /* config the parity bit */
  switch (parity_bits)
  {
    /* odd */
    case 'O':
    case 'o':
      newtio.c_cflag |= PARENB;
      newtio.c_cflag |= PARODD;
      break;
    /* even */
    case 'E':
    case 'e':
      newtio.c_cflag |= PARENB;
      newtio.c_cflag &= ~PARODD;
      break;
    /* none */
    case 'N':
    case 'n':
      newtio.c_cflag &= ~PARENB;
      break;
    default:
      newtio.c_cflag &= ~PARENB;
      break; //8N1 default config
  }
  /* config baudrate */
  j = sizeof(std_rate) / 4;
  for (i = 0; i < j; ++i)
  {
    if (std_rate[i] == baudrate)
    {
      /* set standard baudrate */
      cfsetispeed(&newtio, st_baud[i]);
      cfsetospeed(&newtio, st_baud[i]);
      break;
    }
  }
  /* config stop bit */
  if (stop_bits == 1)
    newtio.c_cflag &= ~CSTOPB;
  else if (stop_bits == 2)
    newtio.c_cflag |= CSTOPB;
  else
    newtio.c_cflag &= ~CSTOPB; //8N1 default config

/* config waiting time & min number of char */
//! If you just want to see if there is data on the line, put the serial config
//! in an unconditional timeout state
#if __x86_64__
  if (testForData)
  {
    newtio.c_cc[VTIME] = 8;
    newtio.c_cc[VMIN]  = 0;
  }
  else
  {
    newtio.c_cc[VTIME] = 1;
    newtio.c_cc[VMIN]  = 18;
  }
#endif
  /* using the raw data mode */
  newtio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  newtio.c_oflag &= ~OPOST;

  /* flush the hardware fifo */
  tcflush(m_serial_fd, TCIFLUSH);

  /* activite the configuration */
  if ((tcsetattr(m_serial_fd, TCSANOW, &newtio)) != 0)
  {
    // DERROR("failed to activate serial configuration\n");
    return false;
  }
  return true;
}

int
LinuxSerialDevice::_serialStart(const char* dev_name, int baud_rate)
{
  const char* ptemp;
  if (dev_name == NULL)
  {
    ptemp = "/dev/ttyUSB0";
  }
  else
  {
    ptemp = dev_name;
  }
  if (true == _serialOpen(ptemp) && true == _serialConfig(baud_rate, 8, 'N', 1))
  {
    FD_ZERO(&m_serial_fd_set);
    FD_SET(m_serial_fd, &m_serial_fd_set);
    return m_serial_fd;
  }
  return -1;
}

int
LinuxSerialDevice::_serialStartOdd(const char* dev_name, int baud_rate)
{
  const char* ptemp;
  if (dev_name == NULL)
  {
    ptemp = "/dev/ttyUSB0";
  }
  else
  {
    ptemp = dev_name;
  }
  if (true == _serialOpen(ptemp) && true == _serialConfig(baud_rate, 8, 'O', 1))
  {

    FD_ZERO(&m_serial_fd_set);
    FD_SET(m_serial_fd, &m_serial_fd_set);

    return m_serial_fd;
  }
  return -1;
}

int
LinuxSerialDevice::_serialWrite(const uint8_t* buf, int len)
{
  return write(m_serial_fd, buf, len);
}


int
LinuxSerialDevice::_serialRead(uint8_t* buf, int len)
{
  int ret = -1;

  if (NULL == buf)
  {
    return -1;
  }
  else
  {
    ret = read(m_serial_fd, buf, len);
    return ret;
  }
}

bool NX_serial_init(LinuxSerialDevice* temp)
{
  temp->init();
  if(temp->getDeviceStatus() == false)
      return false;
  else return true;
}


void NX_serial_read_raw_data(LinuxSerialDevice* serial, circleBuf_t* temp)
{
    uint8_t radio_read_buf[256];
    uint8_t cnt = serial->readall(radio_read_buf,256);
    uint16_t head_n = 0;
    if(cnt != 0){
        for(uint8_t i=0; i<cnt; ++i){
            //! 取得下一个环形缓冲区的头指针
            //! 并判断是否头指针追上了尾指针
            head_n = (temp->head+1)%CIRCLE_BUF_SIZE;
            if(head_n != temp->tail){
                temp->buf[temp->head] = radio_read_buf[i];
                temp->head = head_n;
                // printf("radio_read_buf[%d] = %x\n",i,radio_read_buf[i]);

            }
        }
    }
}

void NX_serial_read_SBUS_raw_data(LinuxSerialDevice* serial, circleBuf_t* temp)
{
    uint8_t SBUS_read_buf[256];
    uint8_t cnt = serial->readall(SBUS_read_buf,256);
    uint16_t head_n = 0;

    // printf("cnt = %d\n",cnt);
    // for(uint8_t k = 0;k<cnt;k++)
    // {
    //   printf("SBUS_read_buf[%d] = %x\n",k,SBUS_read_buf[k]);
    // }
    if(cnt != 0){
        for(uint8_t i=0; i<cnt; ++i){
            //! 取得下一个环形缓冲区的头指针
            //! 并判断是否头指针追上了尾指针
            head_n = (temp->head+1)%CIRCLE_BUF_SIZE;
            if(head_n != temp->tail){
                temp->buf[temp->head] = SBUS_read_buf[i];
                temp->head = head_n;
            }
            //printf("SBUS_read_buf[%d] = %x\n",i,SBUS_read_buf[i]);
        }
        //printf("SBUS_read_buf = %d\n",SBUS_read_buf[2]);
    }

}



void NX_serial_read_flight_control_raw_data(LinuxSerialDevice* serial, circleBuf_t* temp)
{
    uint8_t flightcontrol_read_buf[128];
    uint8_t cnt = serial->readall(flightcontrol_read_buf,128);
    uint16_t head_n = 0;
    // printf("cnt = %d\n",cnt);
    if(cnt != 0){
        for(uint8_t i=0; i<cnt; ++i){
            //! 取得下一个环形缓冲区的头指针
            //! 并判断是否头指针追上了尾指针
            head_n = (temp->head+1)%CIRCLE_BUF_SIZE;
            if(head_n != temp->tail){
                temp->buf[temp->head] = flightcontrol_read_buf[i];
                temp->head = head_n;
                // printf("flightcontrol_read_buf[%d] = %x\n",i,flightcontrol_read_buf[i]);
            }
        }
    }
}

void NX_analyse_groundbase_data(circleBuf_t* temp, uint8_t* data)    //机载计算机解析地面站数据
{
    if(temp->tail != temp->head){
         analyse_groundbase(temp, data);
        temp->tail = (temp->tail+1)%CIRCLE_BUF_SIZE;
    }
}

void NX_analyse_SBUS_data(circleBuf_t* temp, uint8_t* data)    //机载计算机解析遥控器SBUS数据
{
    if(temp->tail != temp->head){
         analyse_SBUS(temp, data);
        temp->tail = (temp->tail+1)%CIRCLE_BUF_SIZE;
    }
}

void NX_analyse_flight_control_data(circleBuf_t* temp, uint8_t* data)    //机载计算机解析飞控数据
{
    if(temp->tail != temp->head){
         analyse_flight_control(temp, data);
        temp->tail = (temp->tail+1)%CIRCLE_BUF_SIZE;
    }
}




